require "extend/BaseTablePool"

local clamp = Mathf.Clamp
local asin 	= math.asin
local atan2 = math.atan2
local rad2Deg = Mathf.Rad2Deg

local pi = Mathf.PI
local half_pi = pi * 0.5
local two_pi = 2 * pi
local negativeFlip = -0.0001
local positiveFlip = two_pi - 0.0001

local P = NewClass(BaseTablePool, ClassFromFile(...))
QuaternionEx.SetPool(P)

function P:New()
	return QuaternionEx._New()
end

function P:FromToRotation(from, to)
	local q = self:Create()
	q:SetFromToRotation(from, to)
	return q
end

function P:Copy(a, b)
	if not a or not b then
		return
	end

	a.x = b.x
	a.y = b.y
	a.z = b.z
	a.w = b.w
end

function P:Inverse(a)
	if not a then
		return
	end

	local quat = self:Create()
	quat.x = -a.x
	quat.y = -a.y
	quat.z = -a.z
	quat.w = a.w
	return quat
end

function P:MulNoAlloc(a, lhs, rhs)
	a:Set((((lhs.w * rhs.x) + (lhs.x * rhs.w)) + (lhs.y * rhs.z)) - (lhs.z * rhs.y), (((lhs.w * rhs.y) + (lhs.y * rhs.w)) + (lhs.z * rhs.x)) - (lhs.x * rhs.z), (((lhs.w * rhs.z) + (lhs.z * rhs.w)) + (lhs.x * rhs.y)) - (lhs.y * rhs.x), (((lhs.w * rhs.w) - (lhs.x * rhs.x)) - (lhs.y * rhs.y)) - (lhs.z * rhs.z))	
end

function P:Mul(b, c)
	local a = self:Create()
	self:MulNoAlloc(a, b, c)
	return a
end


function P:Lerp(q1, q2, t)
	t = clamp(t, 0, 1)
	local q = self:Create()
	
	if Quaternion.Dot(q1, q2) < 0 then
		q.x = q1.x + t * (-q2.x -q1.x)
		q.y = q1.y + t * (-q2.y -q1.y)
		q.z = q1.z + t * (-q2.z -q1.z)
		q.w = q1.w + t * (-q2.w -q1.w)
	else
		q.x = q1.x + (q2.x - q1.x) * t
		q.y = q1.y + (q2.y - q1.y) * t
		q.z = q1.z + (q2.z - q1.z) * t
		q.w = q1.w + (q2.w - q1.w) * t
	end	
	
	q:SetNormalize()
	return q
end

function P:Slerp(from, to, t)	
	return QuaternionEx.Slerp(from, to, t)
	-- t = clamp(t, 0, 1)
	-- return self:UnclampedSlerp(from, to, t)
end

function P:UnclampedSlerp(from, to, t)		
	local cosAngle = Quaternion.Dot(from, to)
	
	local needRecycleToQ = false
    if cosAngle < 0 then    
        cosAngle = -cosAngle
        to = self:Create()
        to:Set(-to.x, -to.y, -to.z, -to.w)
        needRecycleToQ = true
    end
    
    local t1, t2
    local quat
    if cosAngle < 0.95 then    
	    local angle 	= acos(cosAngle)
		local sinAngle 	= sin(angle)
        local invSinAngle = 1 / sinAngle
        t1 = sin((1 - t) * angle) * invSinAngle
        t2 = sin(t * angle) * invSinAngle    
		quat = self:Create()
		quat:Set(from.x * t1 + to.x * t2, from.y * t1 + to.y * t2, from.z * t1 + to.z * t2, from.w * t1 + to.w * t2)
    else    
		quat = QuaternionPool:Lerp(from, to, t)
    end

    if needRecycleToQ then
    	self:Recycle(to)
    	to = nil
    end
    return quat
end

function P:Euler(x, y, z)		
	local quat = self:Create()
	quat:SetEuler(x,y,z)
	return quat
end


function P:SanitizeEuler(euler)	
	if euler.x < negativeFlip then
		euler.x = euler.x + two_pi
	elseif euler.x > positiveFlip then
		euler.x = euler.x - two_pi
	end

	if euler.y < negativeFlip then
		euler.y = euler.y + two_pi
	elseif euler.y > positiveFlip then
		euler.y = euler.y - two_pi
	end

	if euler.z < negativeFlip then
		euler.z = euler.z + two_pi
	elseif euler.z > positiveFlip then
		euler.z = euler.z + two_pi
	end
end

function P:GetEulerAngles(q)
	local x = q.x
	local y = q.y
	local z = q.z
	local w = q.w
		
	local check = 2 * (y * z - w * x)
	
	local v = Vector3Pool:Create()
	if check < 0.999 then
		if check > -0.999 then
			v:Set( -asin(check), 
						atan2(2 * (x * z + w * y), 1 - 2 * (x * x + y * y)), 
						atan2(2 * (x * y + w * z), 1 - 2 * (x * x + z * z)))
			self:SanitizeEuler(v)
			v:Mul(rad2Deg)
			return v
		else
			v:Set(half_pi, atan2(2 * (x * y - w * z), 1 - 2 * (y * y + z * z)), 0)
			self:SanitizeEuler(v)
			v:Mul(rad2Deg)
			return v
		end
	else
		v:Set(-half_pi, atan2(-2 * (x * y - w * z), 1 - 2 * (y * y + z * z)), 0)
		self:SanitizeEuler(v)
		v:Mul(rad2Deg)
		return v
	end

	Vector3Pool:Recycle(v)
	v = nil
end

function P:GetEulerYAngle(q)
	local angles = self:GetEulerAngles(q)
	local yAngle = angles.y
	Vector3Pool:Recycle(angles)
	return yAngle
end



function P:Rotation(t)
	local p = self:Create()
	p:Set(t:GetRotation(nil, nil, nil))
	return p
end

function P:SetRotation(t, rot)
	t:SetRotation(rot.x, rot.y, rot.z, rot.w)
end


function P:LocalRotation(t)
	local p = self:Create()
	p:Set(t:GetLocalRotation(nil, nil, nil))
	return p
end

function P:SetLocalRotation(t, rot)
	t:SetLocalRotation(rot.x, rot.y, rot.z, rot.w)
end

